// a prototype for mapping node

#include <assert.h>
#include <vector>

#include <ros/ros.h>
#include <geometry_msgs/Pose2D.h>
#include <sensor_msgs/LaserScan.h>
#include <tf/transform_broadcaster.h>
//#include <tf/tfMessage.h>
//#include <nav_msgs/MapMetaData.h>
//#include <nav_msgs/OccupancyGrid.h>
#include "navigation/IR_conv.h"

#include <Eigen/Dense>

#define FOR(x, a, b) for(int x = (a); x < (b); x++)

using namespace std;
using namespace Eigen;

namespace navigation
{

  class SimpleMapping
  {

    private:

      ros::Subscriber pose_sub_;
      ros::Subscriber ir_sub_;
      //ros::Subscriber sonar_sub_;
      //ros::Publisher map_pub_;
      //ros::Publisher map_meta_pub_;
      ros::Publisher scan_pub_;

      // pose
      Eigen::Vector3f pose_;

      enum {
        FRONT = 0, LEFT = 1, RIGHT = 2,
        L = 0, R = 1,
        F = 0, M = 1, B = 2,
      };

    public:

      SimpleMapping()
      {
        // parameter init
        pose_ << 0, 0, 0; // initial pose
        // uncertainity

        // Read local parameters
        //ros::NodeHandle local_nh("~");

        /* subscriber */
        ros::NodeHandle nh;
        pose_sub_ = nh.subscribe("/robot/pose", 100, &SimpleMapping::update_pose, this);
        ir_sub_ = nh.subscribe("/IR/ir_converted", 1000, &SimpleMapping::register_walls, this);

        /* publisher */
        //map_pub_ = nh.advertise<nav_msgs::OccupancyGrid>("/mapping/map", 10);
        //map_meta_pub_ = nh.advertise<nav_msgs::MapMetaData>("/mapping/map_metadata", 10);
        scan_pub_ = nh.advertise<sensor_msgs::LaserScan>("/mapping/IRscan", 100);

      }

    protected:

      void update_pose(const geometry_msgs::Pose2D::ConstPtr& msg)
      {
        pose_[0] = msg->x;
        pose_[1] = msg->y;
        pose_[2] = msg->theta;

        // @todo publis tfMessage
        static tf::TransformBroadcaster br;
        tf::Transform transform;
        transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
        transform.setRotation( tf::Quaternion(0, 0, msg->theta-1.57) );
        br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/odom", "/robot/base_link"));
      }

      void register_walls(const navigation::IR_conv::ConstPtr &msg)
      {
        MatrixXf ir(3, 3);

        //! convert IR readings
        _convert_IRs(msg, ir);

        //! estimate wall
        Matrix2f walls[3]; // 
        sensor_msgs::LaserScan scan;
        bool pub = false;
        FOR(i, 0, 3) {
          if(estimate_wall(ir, i,  walls[i]) ) {
            _convert_wall_to_scan(walls[i], scan);
            pub = true;
          }
        }
        // DBG
        ROS_INFO_STREAM("FrontWall: " << endl << walls[FRONT]);
        ROS_INFO_STREAM("LeftWall: " << endl << walls[LEFT]);
        ROS_INFO_STREAM("RightWall: " << endl <<  walls[RIGHT]);

        // publish scanner
        if(pub) scan_pub_.publish(scan);
      }

    private:
      void _convert_IRs(const IR_conv::ConstPtr &msg, 
          Eigen::MatrixXf &ir)
      {
        // distance limitation
        const float L_MAX = 50;
        const float S_MAX = 30;
        const float F_OFFSET = 10;
        const float LR_OFFSET = 9;
        ir(FRONT, L) = (msg->IR5 < L_MAX)? msg->IR5+F_OFFSET: -1;
        ir(FRONT, R) = (msg->IR2 < S_MAX)? msg->IR2+F_OFFSET: -1;
        ir(FRONT, 2) = 0.5 * (ir(FRONT, L) + ir(FRONT, R)); // not used
        ir(LEFT, F) = (msg->IR1 < S_MAX)? msg->IR1+LR_OFFSET: -1;
        ir(LEFT, M) = (msg->IR3 < L_MAX)? msg->IR3+LR_OFFSET: -1;
        ir(LEFT, B) = (msg->IR7 < S_MAX)? msg->IR7+LR_OFFSET: -1;
        ir(RIGHT, F) = (msg->IR4 < S_MAX)? msg->IR4+LR_OFFSET: -1;
        ir(RIGHT, M) = (msg->IR6 < L_MAX)? msg->IR6+LR_OFFSET: -1;
        ir(RIGHT, B) = (msg->IR0 < S_MAX)? msg->IR0+LR_OFFSET: -1;


        // some other limitations?
      }

      // add scan points from wall info
      void _convert_wall_to_scan(Eigen::Matrix2f &wall, sensor_msgs::LaserScan &scan)
      {
        const int num_readings = 56;
        const int angle_min = -50;
        const int angle_max = 230;

        scan.header.stamp = ros::Time::now(); // @todo should be synchronized with IRs?
        scan.header.frame_id = "/robot/ir_scan";
        scan.angle_min = _deg2rad(angle_min);
        scan.angle_max = _deg2rad(angle_max);
        scan.angle_increment = (scan.angle_max-scan.angle_min) / (float)num_readings;
        //scan.scan_time = ;
        //scan.time_increment = ;
        scan.range_min = 5.0;
        scan.range_max = 60.0;

        if(static_cast<int>(scan.ranges.size()) < num_readings) 
          scan.ranges.resize(num_readings);
        FOR(i, 0, num_readings) {
          float angle = scan.angle_min + i * scan.angle_increment;
          float ret = _calc_scan_ranges(angle, wall);
          if(ret>0) scan.ranges[i] = ret;
        }
      }

      float _calc_scan_ranges(float angle, Eigen::Matrix2f wall)
      {
        float a1 = atan2(wall(0, 1), wall(0, 0));
        float a2 = atan2(wall(1, 1), wall(1, 0));
        a1 += (a1 < -3.14/2.0)? 3.14*2.0: 0;
        a2 += (a2 < -3.14/2.0)? 3.14*2.0: 0;
        //ROS_INFO("%f, %f", _rad2deg(a1), _rad2deg(a2));
        if(a1 > a2) {
          ROS_WARN("wall conversion: angle error??");
          return -1.0;
        }

        if(angle < a1 || a2 < angle) return -1.0;
        else
        {
          // calculate distance to intersection
          // AX=U
          Vector2f X;
          Matrix2f A;
          Vector2f U;
          A << tan(angle), -1, wall(1, 1)-wall(0, 1), -(wall(1, 0)-wall(0, 0));
          U << 0, -A.row(1)*wall.row(0).transpose();
          X = A.inverse() * U;
          return X.norm();
        }

        // won't come here
        return 0.0;
      }

      bool estimate_wall(Eigen::MatrixXf &_ir, int INDEX, Eigen::Matrix2f &wall)
      {
        VectorXf ir = _ir.row(INDEX);
        if(ir.minCoeff() < 0) {
          ROS_DEBUG("wall estimation error: ir reading incorrect");
          return false;
        }

        // calculate optimized line
        VectorXf y(3);
        switch(INDEX) {
          case FRONT: y << -10, 10, 0; break;
          default: y << 10, 0, -10; break;
        }
        Vector2f coeff; /* a, b */
        coeff << 1, 0;
        float err = 0;
        _calc_optimized_line(ir, y, coeff, err);

        // validate if the line fits the points
        const float ep = 5.0; // tekito
        if(err < ep) 
        {
          if(coeff(0) == 0) wall << coeff(1), 10, coeff(1), -10;
          else wall << (10-coeff(1))/coeff(0), 10, (-10-coeff(1))/coeff(0), -10;
        } 
        else 
        {
          ROS_DEBUG("wall estimation error: fitting error too large");
          return false;
        }

        // transform coordinate
        _transform_coordinate(wall, INDEX);

        return true;
      }

      void _calc_optimized_line(Eigen::VectorXf &x, Eigen::VectorXf &y,
          Eigen::Vector2f &coeff, float &err)
      {
        Vector3f I; I << 1, 1, 1;
        float mean = (x.sum() / x.size());
        float sx2 = (x-mean*I).squaredNorm();
        float sxy = ( (x-mean*I).transpose()*y ).sum();

        if(sx2 == 0) 
        {
          // fitted line: x = mean
          coeff(0) = 0;
          coeff(1) = mean;

          // calculate error
          err += (x-mean*I).squaredNorm();
        } 
        else 
        {
          float a = sxy / sx2;
          float b = - a * mean;
          coeff << a, b;
          // fitted line: y = a*x + b

          // calculate error
          // @todo change it in a strict way?
          err += (x-(y-b*I)/a).squaredNorm();
        }
      }

      void _transform_coordinate(Eigen::Matrix2f &m, int i) {
        Matrix2f A;
        switch(i) {
          case FRONT:
            A << 0, 1, 1, 0;
            m = m * A;
            break;
          case LEFT:
            A << -1, 0, 0, 1;
            m = m * A;
            break;
          case RIGHT:
            Vector2f tmp = m.row(0);
            m.row(0) = m.row(1);
            m.row(1) = tmp;
            break;
        }
      }

      inline float _deg2rad(float deg) { return deg*3.14/180.0; }
      inline float _rad2deg(float rad) { return rad/3.14*180.0; }

  };

} // end of namespace


int main(int argc, char **argv)
{
  ros::init(argc, argv, "simple_mapping");
  navigation::SimpleMapping SimpleMapping;

  ros::spin();
  return 0;
}


